/**
 * Motor
 *
 * Deze klasse bestuurd beide motoren voor de aandrijving.
 *
 * @author Jasper Nevens <jasper@nevensit.be>
 * @date    01/03/11
 */
#include <iostream>
#include <pthread.h>
#include "motors.h"
#include "serial.h"

using namespace std;

/**
 * Constructor
 *
 * Deze initialiseerd de klasse, en de pointer naar de klasse voor de seriele verbinding
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
Motors::Motors(int prt,int br)
{
	cout << "Motor initialize..." << endl;	

	poort=prt;
	baudrate=br;
}

/**
 * Destructor
 *
 * Doet voorlopig niets
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
Motors:: ~Motors(void)
{
	
}

/**
 * setSpeed
 *
 * zet de snelheid en richting van de motors direct. deze wordt geupdate binnen de 10ms normaalgezien.
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
void Motors::setSpeed(int spd1,int dir1,int spd2,int dir2)
{
	int res = OpenComport(poort, baudrate);
	float speed1 = MAXV*spd1*0.01;
	float speed2 = MAXV*spd2*0.01;
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];

	buf[0]=0xD0 | (dir2 * 4) | dir1;
	buf[1] = speed1;		
	buf[2] = speed2;
	res = SendBuf(poort, buf, 3);
	if(res != 1)
	{
		if(spd1 == 0 && spd2 == 0)
			printf("Stopped\n");
		else
			printf("Going forward, speed %d %d\n", (int)speed1,(int)speed2);
	}
	else
		printf("Error while trying to move.\n");
	CloseComport(poort);
}

/**
 * setSpeedAcc
 *
 * zet een target speed voor de motors, deze snelheid zal accelereren naar deze waarde toe gaan.
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
void Motors::setSpeedAcc(int spd1,int dir1,int spd2,int dir2)
{
	int res = OpenComport(poort, baudrate);
	float speed1 = MAXV*spd1*0.01;
	float speed2 = MAXV*spd2*0.01;
	if(res == 1)
	{
		printf("Error while opening comport\n");
		return;
	}

	unsigned char buf[3];

	buf[0]=0xE0 | (dir2 * 4) | dir1;
	buf[1] = speed1;		
	buf[2] = speed2;
	res = SendBuf(poort, buf, 3);
	if(res != 1)
	{
		if(spd1 == 0 && spd2 == 0)
			printf("Stopped\n");
		else
			printf("Going forward, speed %d %d\n", (int)speed1,(int)speed2);
	}
	else
		printf("Error while trying to move.\n");
	CloseComport(poort);
}

/**
 * stopMotors
 *
 * maak de snelheid van de motors geleik aan NULL, gebruikt de functie setSpeed
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
void Motors::stopMotors(void)
{
	setSpeed(0,MOTOR_BRAKE,0,MOTOR_BRAKE);
}

/**
 * setParam
 *
 * zet paramaters op het TReX_JR bordje
 *
 * @author Jasper Nevens <jappe@nevensit.be>
 * @date    01/03/11
 */
void Motors::setParam(unsigned char param,unsigned char value)
{
	unsigned char	cmd,params[COMLEN];
	
	cmd=0xAF;
	params[0]=param;
	params[1]=value;
	params[2]=0x55;
	params[3]=0x2A;
	
	//ms->addCommand(cmd,params,5);
}
